package com.rockwell.sniffyhunter.model;

import java.util.Map;

import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.SnifferState;

/**
 * This class provides the robot for sniffing and scanning for treasure and obstacle
 * The following actions will be performed:
 * <ul>
 * <li>Scan for treasure.
 * <li>Rotate Robot by an index (45 degrees offset).
 * <li>Map the discovered zone, obstacles and maze.
 * <li>Move to next zone for discovery
 * </ul>
 * <p>
 * 
 * @author      Sniffy Software Team
 * @version     1.0
 */

public class SnifferObject extends StateObject {

	private RobotCommandSequencer m_commandSequencer;
	private Map<Integer, IDetector> m_colorDetectors;
	private int indexAngle; //0 through to 360 degrees, with 45 degrees intervals
	private ExitCode m_exitCode;
	private RobotPosition m_robotPosition;
	private IRobotPositionDetector m_positionDetector;	
	private boolean m_clockwise;
	
	private enum ExitCode {
		UNINTIALIZED,
		BASKET_FULL,
		TREASURE_FOUND,
		SNIFF_COMPLETE, 
	}
	
	public SnifferObject(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_commandSequencer.setUseCompass(true);
		m_commandSequencer.setUseRoughTolerance(true);
		m_colorDetectors = robot.m_colorDetectors;
		m_positionDetector = robot.m_positionDetector;
		m_clockwise = true;
	}
	
	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		machineState = SnifferState.START;	
	}

	@Override
	public boolean IsDone() {
		return machineState == SnifferState.DONE;
	}
	
	public IStateMachine start() {
		m_exitCode = ExitCode.UNINTIALIZED;
		indexAngle = 0;
		machineState = SnifferState.SCAN;
		
		if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
			log("Hooray, found a treasure...", 4);
			m_exitCode = ExitCode.TREASURE_FOUND;
			m_clockwise = !m_clockwise;
			machineState = SnifferState.DONE;
			return machineState;
		}
		
		m_robotPosition = m_positionDetector.getPosition();
		if ((m_robotPosition.leftwalldetected || m_robotPosition.leftwall2detected) ||
			(m_robotPosition.rightwalldetected || m_robotPosition.rightwall2detected)) {
			indexAngle = 360;
			machineState = SnifferState.MAP_DISCOVERED_AREA;	
		}
		/*else if (!m_robotPosition.leftwalldetected && !m_robotPosition.leftwall2detected) {
			indexAngle = 0; 
		}
		else if (!m_robotPosition.rightwalldetected && !m_robotPosition.rightwall2detected) {
			indexAngle = 0; 
		}*/
		
		return machineState;
	}
	
	public boolean IsBasketFull() { 
		return m_exitCode == ExitCode.BASKET_FULL;
	}
	
	public boolean IsTreasureFound() {
		return m_exitCode == ExitCode.TREASURE_FOUND;
	}

	public boolean IsSniffCompleted() {
		return m_exitCode == ExitCode.SNIFF_COMPLETE;
	}

	public IStateMachine RotateForFirstScan() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, indexAngle);
		if (returnVal == ReturnResult.Completed) {
			indexAngle += 45;
			log("Rotated to the -90 deg", 4);
			robot.m_northernStarFinder.Rotate(-90);
			machineState = SnifferState.SCAN;
		}
		return machineState;
	}
	
	public IStateMachine Scan() {
		// Check basket status
		if (robot.isBasketFull()) {
			log("Basket is filled to the brim", 4);
			m_exitCode = ExitCode.BASKET_FULL;
			m_clockwise = !m_clockwise;
			machineState = SnifferState.DONE;
			return machineState;
		}
		
		// Check camera & sensors for treasures
		if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
			log("Hooray, found a treasure...", 4);
			m_exitCode = ExitCode.TREASURE_FOUND;
			m_clockwise = !m_clockwise;
			machineState = SnifferState.DONE;
			return machineState;
		}
		
		m_robotPosition = m_positionDetector.getPosition();
		if (m_robotPosition.frontwalldetected) {
			m_exitCode = ExitCode.SNIFF_COMPLETE;
			m_clockwise = !m_clockwise;
			machineState = SnifferState.DONE;
			return machineState;
		}
		else if (m_robotPosition.leftwalldetected || m_robotPosition.leftwall2detected ||
			m_robotPosition.rightwalldetected || m_robotPosition.rightwall2detected) {
			if (m_clockwise) indexAngle += 45; else indexAngle -= 45;
			machineState = SnifferState.SCAN;
		}
			
		if ((m_clockwise && indexAngle >= 360) || (!m_clockwise && indexAngle <= -360)) {
			log("Completed scanning this zone", 4);		
			machineState = SnifferState.MAP_DISCOVERED_AREA;
		}
		else /*no treasure*/ {
			log("Nothing at this angle, rotate to the next offset", 4);
			machineState = SnifferState.INDEX_ROTATE;
		}
		return machineState;
	}

	public IStateMachine IndexRotate() {
		if ((m_clockwise && indexAngle < 360) || (!m_clockwise && indexAngle > -360)) {
			int angleToRotate = m_clockwise ? 45 : -45;
			ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, angleToRotate);
			if (returnVal == ReturnResult.Completed) {
				log("Rotated 45 degrees", 4);
				robot.m_northernStarFinder.Rotate(angleToRotate);
				indexAngle += angleToRotate;
				machineState = SnifferState.SCAN;
				sleep(1000);
			}
		}
		
		return machineState;
	}

	public IStateMachine MapDiscoveredArea() {
		robot.m_northernStarFinder.MapDiscoveredZone();
		int returnVal = 0;
		if (returnVal == 0 /*mapped*/)
			machineState = SnifferState.ROTATE_BACK;
		
		return machineState;
	}

	public IStateMachine RotateBack() {
		//ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnLeft);
		//if (returnVal == ReturnResult.Completed) { /*move to next scan*/
		int returnVal = 0;
		if (returnVal == 0 /*rotated back*/) {
			log("Rotated to 0 again. Time to move ahead", 4);
			robot.m_northernStarFinder.Rotate(-90);
			indexAngle = 0;
			m_exitCode = ExitCode.SNIFF_COMPLETE;
			m_clockwise = !m_clockwise;
			machineState = SnifferState.DONE;
		}
		
		return machineState;
	}
}
